/*
  Sept 2013

  bgc32 Rev -

  Copyright (c) 2013 John Ihlein.  All rights reserved.

  Open Source STM32 Based Brushless Gimbal Controller Software

  Includes code and/or ideas from:

  1)AeroQuad
  2)BaseFlight
  3)CH Robotics
  4)MultiWii
  5)S.O.H. Madgwick
  6)UAVX

  Designed to run on the EvvGC Brushless Gimbal Controller Board

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

///////////////////////////////////////////////////////////////////////////////
/*
 *  fasttrig.c
 *
 *  Created on: Aug 10, 2013
 *      Author: ala42
 */
#include "fastTrig.h"
#include <cmath>

double RadianFormat(double pi)
{
    if (pi >= M_PI)
        return (pi - 2 * M_PI);
    else if (pi < -M_PI)
        return (pi + 2 * M_PI);
    else
        return (pi);
}

double RadianToAngle(double pi)
{
    return (pi / M_TWOPI) * 360;
}

double AngleToRadian(double angle)
{
    return angle / 360 * M_TWOPI;
}

double FastSin(double pi)
{
    if (pi >= 0)
    {
        int ix = ((int)(pi / M_TWOPI * SINARRAYSIZE)) % SINARRAYSIZE;
        return sinData[ix];
    }
    else
    {
        int ix = ((int)(-pi / M_TWOPI * SINARRAYSIZE)) % SINARRAYSIZE;
        return -sinData[ix];
    }
}

double FastSinByAngle(double angle)
{
    double x = angle / 360.0 * M_TWOPI;
    return FastSin(x);
}

const double sinData[SINARRAYSIZE] = {
    0	,
0.006135885	,
0.012271538	,
0.01840673	,
0.024541229	,
0.030674803	,
0.036807223	,
0.042938257	,
0.049067674	,
0.055195244	,
0.061320736	,
0.06744392	,
0.073564564	,
0.079682438	,
0.085797312	,
0.091908957	,
0.09801714	,
0.104121634	,
0.110222207	,
0.116318631	,
0.122410675	,
0.128498111	,
0.134580709	,
0.140658239	,
0.146730475	,
0.152797185	,
0.158858143	,
0.164913121	,
0.170961889	,
0.17700422	,
0.183039888	,
0.189068664	,
0.195090322	,
0.201104635	,
0.207111376	,
0.21311032	,
0.21910124	,
0.225083911	,
0.231058108	,
0.237023606	,
0.24298018	,
0.248927606	,
0.25486566	,
0.260794118	,
0.266712758	,
0.272621356	,
0.27851969	,
0.284407537	,
0.290284677	,
0.296150888	,
0.302005949	,
0.30784964	,
0.313681741	,
0.319502031	,
0.325310292	,
0.331106306	,
0.336889854	,
0.342660717	,
0.34841868	,
0.354163526	,
0.359895037	,
0.365612998	,
0.371317194	,
0.37700741	,
0.382683433	,
0.388345047	,
0.39399204	,
0.3996242	,
0.405241314	,
0.410843171	,
0.41642956	,
0.422000271	,
0.427555094	,
0.433093819	,
0.438616239	,
0.444122145	,
0.44961133	,
0.455083587	,
0.460538711	,
0.465976496	,
0.471396737	,
0.47679923	,
0.482183772	,
0.48755016	,
0.492898192	,
0.498227667	,
0.503538384	,
0.508830143	,
0.514102744	,
0.51935599	,
0.524589683	,
0.529803625	,
0.53499762	,
0.540171473	,
0.545324989	,
0.550457973	,
0.555570233	,
0.560661576	,
0.565731811	,
0.570780746	,
0.575808192	,
0.580813958	,
0.585797858	,
0.590759702	,
0.595699305	,
0.60061648	,
0.605511042	,
0.610382807	,
0.615231591	,
0.620057212	,
0.624859488	,
0.629638239	,
0.634393284	,
0.639124445	,
0.643831543	,
0.648514401	,
0.653172843	,
0.657806694	,
0.662415778	,
0.666999923	,
0.671558955	,
0.676092704	,
0.680600998	,
0.685083668	,
0.689540545	,
0.693971461	,
0.69837625	,
0.702754745	,
0.707106781	,
0.711432196	,
0.715730826	,
0.720002508	,
0.724247083	,
0.728464391	,
0.732654272	,
0.736816569	,
0.740951126	,
0.745057786	,
0.749136395	,
0.753186799	,
0.757208847	,
0.761202386	,
0.765167266	,
0.769103338	,
0.773010454	,
0.776888466	,
0.780737229	,
0.784556597	,
0.788346428	,
0.792106578	,
0.795836905	,
0.799537269	,
0.803207532	,
0.806847554	,
0.810457199	,
0.81403633	,
0.817584813	,
0.821102515	,
0.824589303	,
0.828045046	,
0.831469613	,
0.834862875	,
0.838224706	,
0.841554978	,
0.844853565	,
0.848120345	,
0.851355193	,
0.854557989	,
0.85772861	,
0.860866939	,
0.863972856	,
0.867046246	,
0.870086991	,
0.873094979	,
0.876070094	,
0.879012227	,
0.881921265	,
0.884797099	,
0.887639621	,
0.890448723	,
0.893224301	,
0.89596625	,
0.898674466	,
0.901348847	,
0.903989293	,
0.906595705	,
0.909167983	,
0.911706032	,
0.914209756	,
0.91667906	,
0.919113852	,
0.92151404	,
0.923879533	,
0.926210242	,
0.928506081	,
0.930766961	,
0.932992799	,
0.93518351	,
0.937339012	,
0.939459224	,
0.941544065	,
0.943593458	,
0.945607326	,
0.947585591	,
0.949528181	,
0.951435021	,
0.953306041	,
0.955141168	,
0.956940336	,
0.958703475	,
0.96043052	,
0.962121404	,
0.963776066	,
0.965394442	,
0.966976471	,
0.968522094	,
0.970031253	,
0.971503891	,
0.972939952	,
0.974339383	,
0.97570213	,
0.977028143	,
0.978317371	,
0.979569766	,
0.980785281	,
0.981963869	,
0.983105488	,
0.984210092	,
0.985277642	,
0.986308097	,
0.987301418	,
0.988257568	,
0.98917651	,
0.99005821	,
0.990902636	,
0.991709754	,
0.992479535	,
0.993211949	,
0.99390697	,
0.994564571	,
0.995184727	,
0.995767415	,
0.996312612	,
0.996820299	,
0.997290457	,
0.997723067	,
0.998118113	,
0.998475581	,
0.998795456	,
0.999077728	,
0.999322385	,
0.999529418	,
0.999698819	,
0.999830582	,
0.999924702	,
0.999981175	,
1	,
0.999981175	,
0.999924702	,
0.999830582	,
0.999698819	,
0.999529417	,
0.999322385	,
0.999077728	,
0.998795456	,
0.998475581	,
0.998118113	,
0.997723067	,
0.997290457	,
0.996820299	,
0.996312612	,
0.995767414	,
0.995184727	,
0.994564571	,
0.99390697	,
0.993211949	,
0.992479535	,
0.991709754	,
0.990902635	,
0.99005821	,
0.98917651	,
0.988257568	,
0.987301418	,
0.986308097	,
0.985277642	,
0.984210092	,
0.983105487	,
0.981963869	,
0.98078528	,
0.979569766	,
0.978317371	,
0.977028142	,
0.97570213	,
0.974339383	,
0.972939952	,
0.971503891	,
0.970031253	,
0.968522094	,
0.966976471	,
0.965394441	,
0.963776066	,
0.962121404	,
0.960430519	,
0.958703475	,
0.956940335	,
0.955141168	,
0.95330604	,
0.951435021	,
0.94952818	,
0.947585591	,
0.945607325	,
0.943593458	,
0.941544065	,
0.939459223	,
0.937339012	,
0.93518351	,
0.932992799	,
0.930766961	,
0.92850608	,
0.926210242	,
0.923879532	,
0.921514039	,
0.919113851	,
0.91667906	,
0.914209755	,
0.911706032	,
0.909167983	,
0.906595704	,
0.903989293	,
0.901348847	,
0.898674465	,
0.895966249	,
0.893224301	,
0.890448723	,
0.88763962	,
0.884797098	,
0.881921264	,
0.879012226	,
0.876070094	,
0.873094978	,
0.870086991	,
0.867046245	,
0.863972856	,
0.860866938	,
0.85772861	,
0.854557988	,
0.851355193	,
0.848120344	,
0.844853565	,
0.841554977	,
0.838224705	,
0.834862874	,
0.831469612	,
0.828045045	,
0.824589302	,
0.821102514	,
0.817584813	,
0.814036329	,
0.810457198	,
0.806847553	,
0.803207531	,
0.799537269	,
0.795836904	,
0.792106577	,
0.788346427	,
0.784556597	,
0.780737228	,
0.776888465	,
0.773010453	,
0.769103337	,
0.765167265	,
0.761202385	,
0.757208846	,
0.753186798	,
0.749136394	,
0.745057785	,
0.740951125	,
0.736816568	,
0.732654271	,
0.72846439	,
0.724247082	,
0.720002507	,
0.715730825	,
0.711432195	,
0.70710678	,
0.702754744	,
0.698376249	,
0.69397146	,
0.689540544	,
0.685083667	,
0.680600997	,
0.676092703	,
0.671558954	,
0.666999921	,
0.662415777	,
0.657806692	,
0.653172842	,
0.6485144	,
0.643831542	,
0.639124444	,
0.634393283	,
0.629638238	,
0.624859487	,
0.620057211	,
0.61523159	,
0.610382805	,
0.605511041	,
0.600616478	,
0.595699304	,
0.590759701	,
0.585797857	,
0.580813957	,
0.57580819	,
0.570780745	,
0.56573181	,
0.560661575	,
0.555570232	,
0.550457972	,
0.545324987	,
0.540171472	,
0.534997619	,
0.529803624	,
0.524589682	,
0.519355989	,
0.514102743	,
0.508830142	,
0.503538383	,
0.498227666	,
0.492898191	,
0.487550159	,
0.482183771	,
0.476799229	,
0.471396736	,
0.465976495	,
0.46053871	,
0.455083586	,
0.449611329	,
0.444122143	,
0.438616237	,
0.433093818	,
0.427555092	,
0.42200027	,
0.416429559	,
0.41084317	,
0.405241313	,
0.399624199	,
0.393992039	,
0.388345046	,
0.382683431	,
0.377007409	,
0.371317193	,
0.365612997	,
0.359895035	,
0.354163524	,
0.348418679	,
0.342660716	,
0.336889852	,
0.331106305	,
0.325310291	,
0.31950203	,
0.313681739	,
0.307849639	,
0.302005948	,
0.296150887	,
0.290284676	,
0.284407536	,
0.278519688	,
0.272621354	,
0.266712756	,
0.260794117	,
0.254865658	,
0.248927604	,
0.242980179	,
0.237023605	,
0.231058107	,
0.22508391	,
0.219101239	,
0.213110319	,
0.207111375	,
0.201104634	,
0.195090321	,
0.189068663	,
0.183039887	,
0.177004219	,
0.170961887	,
0.164913119	,
0.158858142	,
0.152797184	,
0.146730473	,
0.140658238	,
0.134580707	,
0.128498109	,
0.122410674	,
0.11631863	,
0.110222206	,
0.104121633	,
0.098017139	,
0.091908955	,
0.085797311	,
0.079682437	,
0.073564562	,
0.067443918	,
0.061320735	,
0.055195243	,
0.049067673	,
0.042938256	,
0.036807222	,
0.030674802	,
0.024541227	,
0.018406729	,
0.012271537	,
0.006135883	,
-1.41E-09	,
-0.006135886	,
-0.01227154	,
-0.018406731	,
-0.02454123	,
-0.030674805	,
-0.036807224	,
-0.042938258	,
-0.049067676	,
-0.055195246	,
-0.061320738	,
-0.067443921	,
-0.073564565	,
-0.079682439	,
-0.085797314	,
-0.091908958	,
-0.098017142	,
-0.104121635	,
-0.110222209	,
-0.116318632	,
-0.122410677	,
-0.128498112	,
-0.13458071	,
-0.140658241	,
-0.146730476	,
-0.152797187	,
-0.158858145	,
-0.164913122	,
-0.17096189	,
-0.177004222	,
-0.183039889	,
-0.189068666	,
-0.195090323	,
-0.201104636	,
-0.207111378	,
-0.213110321	,
-0.219101242	,
-0.225083913	,
-0.23105811	,
-0.237023607	,
-0.242980181	,
-0.248927607	,
-0.254865661	,
-0.260794119	,
-0.266712759	,
-0.272621357	,
-0.278519691	,
-0.284407539	,
-0.290284679	,
-0.29615089	,
-0.302005951	,
-0.307849642	,
-0.313681742	,
-0.319502032	,
-0.325310294	,
-0.331106307	,
-0.336889855	,
-0.342660719	,
-0.348418682	,
-0.354163527	,
-0.359895038	,
-0.365612999	,
-0.371317195	,
-0.377007412	,
-0.382683434	,
-0.388345048	,
-0.393992042	,
-0.399624201	,
-0.405241315	,
-0.410843173	,
-0.416429562	,
-0.422000272	,
-0.427555095	,
-0.43309382	,
-0.43861624	,
-0.444122146	,
-0.449611331	,
-0.455083589	,
-0.460538712	,
-0.465976497	,
-0.471396738	,
-0.476799231	,
-0.482183774	,
-0.487550162	,
-0.492898194	,
-0.498227668	,
-0.503538385	,
-0.508830144	,
-0.514102746	,
-0.519355992	,
-0.524589684	,
-0.529803626	,
-0.534997621	,
-0.540171474	,
-0.54532499	,
-0.550457974	,
-0.555570234	,
-0.560661578	,
-0.565731812	,
-0.570780747	,
-0.575808193	,
-0.580813959	,
-0.585797859	,
-0.590759703	,
-0.595699306	,
-0.600616481	,
-0.605511043	,
-0.610382808	,
-0.615231592	,
-0.620057213	,
-0.624859489	,
-0.62963824	,
-0.634393285	,
-0.639124446	,
-0.643831544	,
-0.648514402	,
-0.653172844	,
-0.657806695	,
-0.662415779	,
-0.666999924	,
-0.671558956	,
-0.676092705	,
-0.680600999	,
-0.685083669	,
-0.689540546	,
-0.693971462	,
-0.698376251	,
-0.702754746	,
-0.707106782	,
-0.711432197	,
-0.715730827	,
-0.720002509	,
-0.724247084	,
-0.728464392	,
-0.732654273	,
-0.73681657	,
-0.740951127	,
-0.745057787	,
-0.749136396	,
-0.7531868	,
-0.757208848	,
-0.761202387	,
-0.765167267	,
-0.769103339	,
-0.773010455	,
-0.776888467	,
-0.78073723	,
-0.784556598	,
-0.788346429	,
-0.792106578	,
-0.795836906	,
-0.79953727	,
-0.803207533	,
-0.806847555	,
-0.810457199	,
-0.814036331	,
-0.817584814	,
-0.821102516	,
-0.824589304	,
-0.828045046	,
-0.831469613	,
-0.834862876	,
-0.838224707	,
-0.841554978	,
-0.844853566	,
-0.848120346	,
-0.851355194	,
-0.854557989	,
-0.857728611	,
-0.86086694	,
-0.863972857	,
-0.867046246	,
-0.870086992	,
-0.873094979	,
-0.876070095	,
-0.879012227	,
-0.881921265	,
-0.884797099	,
-0.887639621	,
-0.890448724	,
-0.893224302	,
-0.895966251	,
-0.898674467	,
-0.901348848	,
-0.903989294	,
-0.906595705	,
-0.909167984	,
-0.911706033	,
-0.914209756	,
-0.916679061	,
-0.919113852	,
-0.92151404	,
-0.923879533	,
-0.926210243	,
-0.928506081	,
-0.930766962	,
-0.9329928	,
-0.935183511	,
-0.937339013	,
-0.939459224	,
-0.941544066	,
-0.943593459	,
-0.945607326	,
-0.947585592	,
-0.949528181	,
-0.951435022	,
-0.953306041	,
-0.955141169	,
-0.956940336	,
-0.958703475	,
-0.96043052	,
-0.962121405	,
-0.963776066	,
-0.965394442	,
-0.966976472	,
-0.968522095	,
-0.970031254	,
-0.971503891	,
-0.972939953	,
-0.974339383	,
-0.97570213	,
-0.977028143	,
-0.978317371	,
-0.979569766	,
-0.980785281	,
-0.981963869	,
-0.983105488	,
-0.984210093	,
-0.985277643	,
-0.986308098	,
-0.987301418	,
-0.988257568	,
-0.98917651	,
-0.990058211	,
-0.990902636	,
-0.991709754	,
-0.992479535	,
-0.993211949	,
-0.99390697	,
-0.994564571	,
-0.995184727	,
-0.995767415	,
-0.996312612	,
-0.996820299	,
-0.997290457	,
-0.997723067	,
-0.998118113	,
-0.998475581	,
-0.998795456	,
-0.999077728	,
-0.999322385	,
-0.999529418	,
-0.999698819	,
-0.999830582	,
-0.999924702	,
-0.999981175	,
-1	,
-0.999981175	,
-0.999924702	,
-0.999830582	,
-0.999698819	,
-0.999529417	,
-0.999322385	,
-0.999077728	,
-0.998795456	,
-0.99847558	,
-0.998118113	,
-0.997723066	,
-0.997290457	,
-0.996820299	,
-0.996312612	,
-0.995767414	,
-0.995184726	,
-0.994564571	,
-0.99390697	,
-0.993211949	,
-0.992479534	,
-0.991709753	,
-0.990902635	,
-0.99005821	,
-0.98917651	,
-0.988257567	,
-0.987301418	,
-0.986308097	,
-0.985277642	,
-0.984210092	,
-0.983105487	,
-0.981963869	,
-0.98078528	,
-0.979569765	,
-0.97831737	,
-0.977028142	,
-0.97570213	,
-0.974339382	,
-0.972939952	,
-0.97150389	,
-0.970031253	,
-0.968522094	,
-0.96697647	,
-0.965394441	,
-0.963776065	,
-0.962121404	,
-0.960430519	,
-0.958703474	,
-0.956940335	,
-0.955141168	,
-0.95330604	,
-0.95143502	,
-0.94952818	,
-0.94758559	,
-0.945607325	,
-0.943593457	,
-0.941544064	,
-0.939459223	,
-0.937339011	,
-0.935183509	,
-0.932992798	,
-0.93076696	,
-0.92850608	,
-0.926210241	,
-0.923879532	,
-0.921514038	,
-0.919113851	,
-0.916679059	,
-0.914209755	,
-0.911706031	,
-0.909167982	,
-0.906595704	,
-0.903989292	,
-0.901348846	,
-0.898674465	,
-0.895966249	,
-0.8932243	,
-0.890448722	,
-0.887639619	,
-0.884797097	,
-0.881921263	,
-0.879012225	,
-0.876070093	,
-0.873094977	,
-0.87008699	,
-0.867046244	,
-0.863972855	,
-0.860866937	,
-0.857728609	,
-0.854557987	,
-0.851355192	,
-0.848120344	,
-0.844853564	,
-0.841554976	,
-0.838224704	,
-0.834862874	,
-0.831469611	,
-0.828045044	,
-0.824589301	,
-0.821102514	,
-0.817584812	,
-0.814036328	,
-0.810457197	,
-0.806847552	,
-0.80320753	,
-0.799537268	,
-0.795836903	,
-0.792106576	,
-0.788346426	,
-0.784556596	,
-0.780737227	,
-0.776888464	,
-0.773010452	,
-0.769103336	,
-0.765167264	,
-0.761202384	,
-0.757208845	,
-0.753186797	,
-0.749136393	,
-0.745057784	,
-0.740951124	,
-0.736816567	,
-0.73265427	,
-0.728464389	,
-0.724247081	,
-0.720002506	,
-0.715730824	,
-0.711432194	,
-0.707106779	,
-0.702754743	,
-0.698376248	,
-0.693971459	,
-0.689540543	,
-0.685083666	,
-0.680600996	,
-0.676092702	,
-0.671558953	,
-0.66699992	,
-0.662415776	,
-0.657806691	,
-0.653172841	,
-0.648514399	,
-0.643831541	,
-0.639124443	,
-0.634393282	,
-0.629638237	,
-0.624859486	,
-0.62005721	,
-0.615231589	,
-0.610382804	,
-0.605511039	,
-0.600616477	,
-0.595699302	,
-0.5907597	,
-0.585797855	,
-0.580813956	,
-0.575808189	,
-0.570780744	,
-0.565731809	,
-0.560661574	,
-0.555570231	,
-0.550457971	,
-0.545324986	,
-0.540171471	,
-0.534997618	,
-0.529803623	,
-0.52458968	,
-0.519355988	,
-0.514102742	,
-0.50883014	,
-0.503538381	,
-0.498227665	,
-0.49289819	,
-0.487550158	,
-0.48218377	,
-0.476799228	,
-0.471396735	,
-0.465976493	,
-0.460538709	,
-0.455083585	,
-0.449611327	,
-0.444122142	,
-0.438616236	,
-0.433093816	,
-0.427555091	,
-0.422000268	,
-0.416429558	,
-0.410843169	,
-0.405241312	,
-0.399624197	,
-0.393992038	,
-0.388345044	,
-0.38268343	,
-0.377007408	,
-0.371317191	,
-0.365612995	,
-0.359895034	,
-0.354163523	,
-0.348418678	,
-0.342660715	,
-0.336889851	,
-0.331106303	,
-0.32531029	,
-0.319502028	,
-0.313681738	,
-0.307849637	,
-0.302005947	,
-0.296150886	,
-0.290284675	,
-0.284407535	,
-0.278519687	,
-0.272621353	,
-0.266712755	,
-0.260794115	,
-0.254865657	,
-0.248927603	,
-0.242980177	,
-0.237023603	,
-0.231058106	,
-0.225083909	,
-0.219101238	,
-0.213110317	,
-0.207111374	,
-0.201104632	,
-0.195090319	,
-0.189068661	,
-0.183039885	,
-0.177004218	,
-0.170961886	,
-0.164913118	,
-0.158858141	,
-0.152797183	,
-0.146730472	,
-0.140658237	,
-0.134580706	,
-0.128498108	,
-0.122410672	,
-0.116318628	,
-0.110222205	,
-0.104121631	,
-0.098017138	,
-0.091908954	,
-0.08579731	,
-0.079682435	,
-0.073564561	,
-0.067443917	,
-0.061320734	,
-0.055195242	,
-0.049067672	,
-0.042938254	,
-0.03680722	,
-0.0306748	,
-0.024541226	,
-0.018406727	,
-0.012271535	,
-0.006135882	,

};
